% Devin Koepl

clear('all');
close('all');
addpath('../animations', '../controllers', '../dynamics', '../environments', '../library');

phases = 20;
output = cell(phases, 2);

x0 = [ 0, 0*3, 1, 0, 0, 0, pi/2+0*0.642774628*pi, 0, ForceControlModelClass.r0 * ForceControlModelClass.G / ForceControlModelClass.rl, 0];

% controller = ComControllerClass;
controller = OptTorqueControllerClass;

controller.time_vector = linspace(0,0.4,10);
controller.torque_vector = 0*[0 0 0 0.25 0 0.5 1 1 1 1];

% @flat_rigid_environment

sim = ForceControlModelClass(x0, @flat_rigid_environment, controller);

for i = 1 : phases
   [output{i, 1}, output{i, 2}] = sim.run(); 
end

figure;
title('CoM Height');
xlabel('Time (s)');
ylabel('Height (m)');
hold on;
grid on
for i = 1 : phases
    plot(output{i, 1}, output{i, 2}(:, sim.yci), 'linewidth', 3);
end

figure;
title('CoM Velocity');
xlabel('Time (s)');
ylabel('Velocity (m/s)');
hold on;
grid on
for i = 1 : phases
    plot(output{i, 1}, output{i, 2}(:, sim.dxci), 'linewidth', 3);
end

height_error = output{3, 2}(end,sim.yci) - output{1, 2}(1,sim.yci)
vel_error = output{3, 2}(end,sim.dxci) - output{1, 2}(1,sim.dxci)


figure;
title('Gear Angle');
xlabel('Time (s)');
ylabel('Angle');
hold on;
grid on
for i = 2 : 2
    plot(output{i, 1}, output{i, 2}(:, sim.oli), 'linewidth', 3);
end


% figure;
% title('Leg Motor Angle');
% xlabel('Time (s)');
% ylabel('Angle (rad)');
% hold on;
% for i = 1 : phases
%     plot(output{i, 1}, output{i, 2}(:, sim.oli), 'linewidth', 3);
% end

% Now animate the output.
anim = AnimateForceControlModelClass(sim.gnd, output);
anim.animate;
